/*----------------------------------------------------------------------------*/
/* Copyright (c) 2012 Worthington Kilbourne Robot Club. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package us.oh.k12.wkw.robot;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.buttons.NetworkButton;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Date;
import us.oh.k12.wkw.robot.command.ClimbingTiltPlungerDownCmd;
import us.oh.k12.wkw.robot.command.ClimbingTiltPlungerUpCmd;
import us.oh.k12.wkw.robot.command.DriveSmallLeftTurnCmd;
import us.oh.k12.wkw.robot.command.DriveSmallRightTurnCmd;
import us.oh.k12.wkw.robot.command.DriveToggleXPidCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleGoDownCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleGoUpCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleIncreaseSpeedCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleStopCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleToCornerPyramidCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleToFeedCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleToFrontOfPyramidCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleToOffsetPyramidCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleToPyramidShotCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleToZeroCmd;
import us.oh.k12.wkw.robot.command.LauncherChangeYOffsetCmd;
import us.oh.k12.wkw.robot.command.LauncherFeedFrisbeeCmd;
import us.oh.k12.wkw.robot.command.LauncherFire4Cmd;
import us.oh.k12.wkw.robot.command.LauncherFireCmd;
import us.oh.k12.wkw.robot.command.LauncherMotorStopCmd;
import us.oh.k12.wkw.robot.command.LauncherSpeedIncreaseCmd;
import us.oh.k12.wkw.robot.command.LauncherStopFireCmd;
import us.oh.k12.wkw.robot.command.LauncherToggleAnglePidCmd;
import us.oh.k12.wkw.robot.command.StartCompressorCmd;
import us.oh.k12.wkw.robot.command.StopCompressorCmd;
import us.oh.k12.wkw.robot.util.SimpleDateFormat;

/**
 * Represents an operator interface.<br> <br> The joystick buttons:<br 1 =
 * trigger<br> 2 = top rear<br> 3 = top center<br> 4 = top left<br> 5 = top
 * right<br> 6, 7 = base left side<br> 8, 9 - base back side<br> 10, 11 - base
 * right side.
 *
 */
public class OperatorInterface {

    private Joystick leftJoystick;
    private Joystick rightJoystick;
    private Joystick launcherJoystick;
    private NetworkButton fireOneButton;
    private NetworkButton fireFourButton;
    private NetworkButton toggleAnglePidButton;
    private NetworkButton angleToZeroButton;
    private NetworkButton tiltUpButton;
    private NetworkButton tiltDownButton;

    public OperatorInterface() {

        super();

        try {

            this.setupLeftJoystick();

            this.setupRightJoystick();

            this.setupLauncherJoystick();

            System.out.println("OperatorInterface initialized.");

        } catch (Exception anEx) {
            this.error("OperatorInterface()", anEx);
        }
    }

    public Joystick getLeftJoystick() {
        return this.leftJoystick;
    }

    public Joystick getRightJoystick() {
        return this.rightJoystick;
    }

    public Joystick getLauncherJoystick() {
        return this.launcherJoystick;
    }

    private void setupLeftJoystick() {

        try {

            this.leftJoystick = new Joystick(3);
            
            if (null != this.leftJoystick) {
                
                // turn left a very small amount
                new JoystickButton(this.leftJoystick, 1)
                        .whenPressed(new DriveSmallLeftTurnCmd());
                
                
                new JoystickButton(this.leftJoystick, 2)
                        .whenPressed(new LauncherFeedFrisbeeCmd());
                
                 new JoystickButton(this.leftJoystick, 3)
                        .whenPressed(new LauncherAngleToPyramidShotCmd());
                
                 
                    
                 new JoystickButton(this.leftJoystick, 6)
                        .whenPressed(new LauncherAngleToOffsetPyramidCmd());
                 
                 
                 new JoystickButton(this.leftJoystick, 7)
                        .whenPressed(new LauncherAngleToFrontOfPyramidCmd());
          
                 
                 new JoystickButton(this.leftJoystick, 10)
                        .whenPressed(new LauncherAngleToCornerPyramidCmd());
                
            }

        } catch (Exception anEx) {
            this.error("setupLeftJoystick()", anEx);
        }
    }

    private void setupRightJoystick() {

        try {

            this.rightJoystick = new Joystick(2);

            if (null != this.rightJoystick) {
                
                // turn right a very small amount
                new JoystickButton(this.rightJoystick, 1)
                        .whenPressed(new DriveSmallRightTurnCmd());

                //2 -Toggle X-PID on/off
                new JoystickButton(this.rightJoystick, 2)
                        .whileHeld(new DriveToggleXPidCmd());
                
               // new JoystickButton(this.rightJoystick, 2)
                      //  .whenReleased(new DriveXPidDisableCmd());

                //6 -Turn compressor on
                new JoystickButton(this.rightJoystick, 6)
                        .whenPressed(new StartCompressorCmd());

                //7 -Turn compressor off
                new JoystickButton(this.rightJoystick, 7)
                        .whenPressed(new StopCompressorCmd());

                //8 -Decrease launcher motor speed
                new JoystickButton(this.rightJoystick, 8)
                        .whenPressed(new LauncherSpeedIncreaseCmd(-.05));

                //9 -Increase launcher motor speed
                new JoystickButton(this.rightJoystick, 9)
                        .whenPressed(new LauncherSpeedIncreaseCmd(.05));

                //10 -Decrease angle motor speed]
                new JoystickButton(this.rightJoystick, 10)
                        .whenPressed(new LauncherAngleIncreaseSpeedCmd(-.02));

                //11 -Increase angle motor speed
                new JoystickButton(this.rightJoystick, 11)
                        .whenPressed(new LauncherAngleIncreaseSpeedCmd(.02));

            }

        } catch (Exception anEx) {
            this.error("setupGamepad()", anEx);
        }
    }

    private void setupLauncherJoystick() {

        try {

            this.launcherJoystick = new Joystick(1);

            if (null != this.launcherJoystick) {

                //1 -While held, Fires 4 Frisbees
                new JoystickButton(this.launcherJoystick, 1)
                        .whenPressed(new LauncherFire4Cmd());
                
                 new JoystickButton(this.launcherJoystick, 1)
                        .whenReleased(new LauncherStopFireCmd());
                
                this.fireFourButton = new NetworkButton(NetworkTable.getTable("SmartDashboard"), "Fire 4 Frisbees");
                this.fireFourButton.whenPressed(new LauncherFire4Cmd());
                SmartDashboard.putData("Fire 4 Frisbees", this.fireFourButton);

                //2 -Toggle Y-PID Controller On/Off 
                new JoystickButton(this.launcherJoystick, 2)
                        .whenPressed(new LauncherToggleAnglePidCmd());
                this.toggleAnglePidButton = new NetworkButton(NetworkTable.getTable("SmartDashboard"), "Toggle Angle PID");
                this.toggleAnglePidButton.whenPressed(new LauncherToggleAnglePidCmd());
                SmartDashboard.putData("Toggle Angle PID", this.toggleAnglePidButton);

                //3 -Shoot 1 Frisbee
                new JoystickButton(this.launcherJoystick, 3)
                        .whenPressed(new LauncherFireCmd());
                
                new JoystickButton(this.launcherJoystick, 3)
                        .whenReleased(new LauncherStopFireCmd());
                
                this.fireOneButton = new NetworkButton(NetworkTable.getTable("SmartDashboard"), "Fire 1 Frisbee");
                this.fireOneButton.whenPressed(new LauncherFireCmd());
                SmartDashboard.putData("Fire 1 Frisbee", this.fireOneButton);

                //4 -Turn off PID, Set launcher angle to 0 degrees for climbing
                new JoystickButton(this.launcherJoystick, 4)
                        .whenPressed(new LauncherAngleToZeroCmd());
                this.angleToZeroButton = new NetworkButton(NetworkTable.getTable("SmartDashboard"), "Angle to Zero");
                this.angleToZeroButton.whenPressed(new LauncherAngleToZeroCmd());
                SmartDashboard.putData("Angle to Zero", this.angleToZeroButton);

                //5 -Turn off PID, Set launcher angle to 30 degrees for feeding frisbees
                new JoystickButton(this.launcherJoystick, 5)
                        .whenPressed(new LauncherAngleToFeedCmd());

                //6 -Tilts robot up for climbing
                new JoystickButton(this.launcherJoystick, 6)
                        .whenPressed(new ClimbingTiltPlungerUpCmd());
                this.tiltUpButton = new NetworkButton(NetworkTable.getTable("SmartDashboard"), "Tilt Up");
                this.tiltUpButton.whenPressed(new ClimbingTiltPlungerUpCmd());
                SmartDashboard.putData("Tilt Up", this.tiltUpButton);

                //7 -Tilts robot down for climbing
                new JoystickButton(this.launcherJoystick, 7)
                        .whenPressed(new ClimbingTiltPlungerDownCmd());
                this.tiltDownButton = new NetworkButton(NetworkTable.getTable("SmartDashboard"), "Tilt Down");
                this.tiltDownButton.whenPressed(new ClimbingTiltPlungerDownCmd());
                SmartDashboard.putData("Tilt Down", this.tiltDownButton);

                //8 -While held, manual angle launcher down
                new JoystickButton(this.launcherJoystick, 8)
                        .whenPressed(new LauncherAngleGoDownCmd());
                new JoystickButton(this.launcherJoystick, 8)
                        .whenReleased(new LauncherAngleStopCmd());

                //9 -While held, manual angle launcher up
                new JoystickButton(this.launcherJoystick, 9)
                        .whenPressed(new LauncherAngleGoUpCmd());
                new JoystickButton(this.launcherJoystick, 9)
                        .whenReleased(new LauncherAngleStopCmd());

                //10 -Decrease Y-Offest, shooting too low 
                new JoystickButton(this.launcherJoystick, 10)
                        .whenPressed(new LauncherChangeYOffsetCmd(-2));

                //11 -Increase Y-Offset, shooting too high
                new JoystickButton(this.launcherJoystick, 11)
                        .whenPressed(new LauncherChangeYOffsetCmd(2));

            }

        } catch (Exception anEx) {
            this.error("setupGamepad()", anEx);
        }

    }

    /*
     * 
     * 
     * helper methods.
     * 
     */
    protected void debug(final String pMethod, final String pMessage) {
        //WkwFrcLogger.debug(this.getClassName(), pMethod, pMessage);
        System.out.println(pMessage);
    }

    protected void info(final String pMethod, final String pMessage) {
        //WkwFrcLogger.info(this.getClassName(), pMethod, pMessage);
        System.out.println(pMessage);
    }

    private String formatException(final Exception pEx) {
        return (null == pEx ? "." : pEx.getClass().getName() + ", message=" + pEx.getMessage()
                + ".");
    }

    protected void error(final String pMethod, final Exception pEx) {
        this.error(pMethod, this.formatException(pEx), pEx);
    }

    protected void error(final String pMethod, final String pMessage, final Exception pEx) {
        //WkwFrcLogger.error(this.getClassName(), pMethod, pMessage, pEx);
        //if (null != pEx) {
        //    pEx.printStackTrace();
        //}
        System.out.println(pMessage);
    }

    protected String getDateTimeFormatted() {
        final SimpleDateFormat aformat = new SimpleDateFormat();
        return aformat.format(new Date());
    }

    protected String getClassName() {
        String aClassName = this.getClass().getName();
        return aClassName.substring(aClassName.lastIndexOf('.') + 1);
    }
}
